
package edu.wpi.first.wpilibj.templates.subsystems;

import edu.wpi.first.wpilibj.AnalogChannel;
import edu.wpi.first.wpilibj.Relay;
import edu.wpi.first.wpilibj.command.Subsystem;
import edu.wpi.first.wpilibj.Talon;
import edu.wpi.first.wpilibj.Jaguar;
import edu.wpi.first.wpilibj.CANJaguar;
import edu.wpi.first.wpilibj.can.CANTimeoutException;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.templates.RobotMap;
import edu.wpi.first.wpilibj.livewindow.*;
import edu.wpi.first.wpilibj.templates.commands.*;
import edu.wpi.first.wpilibj.command.PIDSubsystem;
import edu.wpi.first.wpilibj.PIDController;

/**
 * @author FRC Team 3255 
 */
public class LinearShooter extends PIDSubsystem {
    // Put methods for controlling this subsystem
    // here. Call these from Commands.

    CANJaguar frontWheel = null;
    CANJaguar middleWheel = null;
    CANJaguar backWheel = null;
    Talon pitchMotor = null;
    Relay photonCannonRelay = null;
    AnalogChannel pot;

    public double currentShooterSpeed = 0.0;
    public double currentPitchSpeed = 0.0;
    public boolean photonCannonState;
    
    private String position = "";

    private static final double MIN_VOLTAGE = 2.7;
    private static final double MAX_VOLTAGE = 3.2;
    
    private static final double STOW_VOLTAGE = 2.440;
    private static final double VERY_NEAR_THREE_POINT_VOLTAGE = 2.543;
    private static final double NEAR_THREE_POINT_VOLTAGE = 2.641;
    private static final double FAR_THREE_POINT_VOLTAGE = 2.821;
    private static final double VERY_FAR_THREE_POINT_VOLTAGE = 2.939;

    private static final int FRONT_POLARITY = 1;
    private static final int MIDDLE_POLARITY = 1;
    private static final int BACK_POLARITY = 1;
    
    public LinearShooter(){
        super("Shooter", 1.0, 0.0, 0.0);
        setInputRange(MIN_VOLTAGE, MAX_VOLTAGE);
        setAbsoluteTolerance(0.01);
        PIDController controller = this.getPIDController();
        controller.setContinuous(false);
        try {
            frontWheel = new CANJaguar(RobotMap.SHOOTER_FRONT_JAGUAR);
            middleWheel = new CANJaguar(RobotMap.SHOOTER_MIDDLE_JAGUAR);
            backWheel = new CANJaguar(RobotMap.SHOOTER_BACK_JAGUAR);
            frontWheel.setSafetyEnabled(false);
            middleWheel.setSafetyEnabled(false);
            backWheel.setSafetyEnabled(false);
            
            pot = new AnalogChannel(RobotMap.SHOOTER_POT);

            pitchMotor = new Talon(RobotMap.SHOOTER_PITCH_TALON);
            pitchMotor.setSafetyEnabled(false);
            
            photonCannonRelay = new Relay(RobotMap.PHOTON_CANNON_RELAY, Relay.Direction.kForward); 
        }
        catch (Exception e){
           e.printStackTrace();
        }
    }
    
    public void initDefaultCommand() {
        this.setDefaultCommand(new DoJoystickShoot());
    }
  
    public double getError() {
        return this.getPIDController().getError();
    }
    
    protected double returnPIDInput() {
        try {
            double potVoltage = pot.getAverageVoltage();
            return potVoltage;
        }
        catch(Exception e) {
            return 0.0;
        }
    }
    
    public double potVoltage() {
        return returnPIDInput();
    }
    
    protected void usePIDOutput(double output) {
        if(output > 0) {
            output = .2;
        }
        else {
            output = -.4;
        }
        setPositionSpeed(-output);
    }
    
    public void setPositionSpeed(double s) {
        try {
            pitchMotor.pidWrite(s);
            currentPitchSpeed = s;
        }
        catch (Exception e) {
        }
    }
    
    public double getPositionSpeed(){
        return currentPitchSpeed;
    }
  
    public void setSpeed(double speed){
        try {
            double frontSpeed = (FRONT_POLARITY*speed);
            double middleSpeed = (MIDDLE_POLARITY*speed);
            double backSpeed = (BACK_POLARITY*speed);
            frontWheel.setX(frontSpeed);
            middleWheel.setX(middleSpeed);
            backWheel.setX(backSpeed);
            
            currentShooterSpeed = speed;
        }
        catch (Exception e){
            e.printStackTrace();
        }
    }
    
    public double currentShooterSpeed() {
        return currentShooterSpeed;
    }
    
     public void setPhotonCannonState(boolean on) {
        if(photonCannonRelay != null) {
            if (on) {
                photonCannonRelay.set(Relay.Value.kOn);
                photonCannonState = true;
            }
            else {
                photonCannonRelay.set(Relay.Value.kOff);
                photonCannonState = false;
            }
        }
    }
    
    public void setPositionVeryNear() {
        this.setSetpoint(VERY_NEAR_THREE_POINT_VOLTAGE);
        position = "Near";
    }
    
    public void setPositionNear() {
        this.setSetpoint(NEAR_THREE_POINT_VOLTAGE);
        position = "Near";
    }
    
    public void setStowPosition() {
        this.setSetpoint(STOW_VOLTAGE);
        position = "Stow";
    }
     
    public void setPositionFar() {
        this.setSetpoint(FAR_THREE_POINT_VOLTAGE);
        position = "Far";
    }
    
    public void setPositionVeryFar() {
        this.setSetpoint(VERY_FAR_THREE_POINT_VOLTAGE);
        position = "Near";
    }
    
    public String getPositionString() {
        return position;
    }
     
    public boolean photonCannonState() {
        return photonCannonState;
    } 
}